#include "target_sender.h"

namespace {
    const int kReadQueueOpWait = 10000; //每次等待10毫秒
    const int kFrameEnQueueRetryTimes = 1000;//为了防止丢帧,入队最多等待 1ms
    const int kSendDataQueueSize = 256;

    const int kUdpSendBufSzie = 1024*10000;
    const unsigned short kport = 12007;      // 下位机发送端口

    const int kBytesOfUdpFrameHead = 20;         // 每个UDP数据包中帧头的大小
    const unsigned int unit_length = 8092;
}

std::tuple<string, int> splitIpPort(std::string addr) {
    
    auto first = addr.find(':');
    // 若找不到':'就返回空
    if (first == addr.npos) {
        return {};
    } 

    auto ip = addr.substr(0, first);
    int port;

    try {
        port = atoi(addr.substr(first+1, addr.size()).c_str());
        if (port < 1000 || port > 65535) {
            return {};
        }
        regex pattern("(25[0-5]|2[0-4]\\d|[0-1]\\d{2}|[1-9]?\\d)\\.(25[0-5]|2[0-4]\\d|[0-1]\\d{2}|[1-9]?\\d)\\.(25[0-5]|2[0-4]\\d|[0-1]\\d{2}|[1-9]?\\d)\\.(25[0-5]|2[0-4]\\d|[0-1]\\d{2}|[1-9]?\\d)");
        smatch res;
        if(!regex_match(ip, res, pattern)){
            return {};
        }
    } catch (std::exception e) {
        return {};
    }
    return {ip, port};
}

TargetSender::TargetSender():
SendDataQueue_(kSendDataQueueSize) 
 {}

 TargetSender::~TargetSender() {
     DestroyResource();
 }

 void TargetSender::DestroyResource() {
    while(SendDataQueue_.size());
    dvpp_.DestroyResource();
}

Result TargetSender::Init(aclrtContext& context, aclrtStream& stream, std::string networkAddr) {
    //创建socket连接
    int optval = 1;
    int nSendBuf = kUdpSendBufSzie;

    auto [ip, port] = splitIpPort(networkAddr);
    if (ip.empty()) {
        ERROR_LOG("fail to parse network address");
        return FAILED;
    }


    clifd = socket(AF_INET, SOCK_DGRAM, 0);
    if(clifd < 0)
    {
        ERROR_LOG("fail to init sender socket");
        return FAILED;
    }

    // 下位机
    struct sockaddr_in server_addr;
    server_addr.sin_family = AF_INET;
    server_addr.sin_addr.s_addr = inet_addr("0.0.0.0");
    server_addr.sin_port = htons(kport);
    int ret = setsockopt(clifd, SOL_SOCKET, SO_REUSEADDR,
                        (const void*)&optval, sizeof (optval));
    if (ret == -1) {
        ERROR_LOG("fail to set SO_REUSEADDR option");
        return FAILED;
    }
    ret = setsockopt(clifd, SOL_SOCKET, SO_SNDBUF,
                        (const void*)&nSendBuf, sizeof(nSendBuf));
    if (ret == -1) {
        ERROR_LOG("fail to set SO_SNDBUF option");
        return FAILED;
    }
    bind(clifd, (struct sockaddr *)&server_addr, sizeof(server_addr));

    // 上位机
    bzero(&client_addr, sizeof(client_addr));
    client_addr.sin_family=AF_INET;
    client_addr.sin_addr.s_addr=inet_addr(ip.c_str());
    client_addr.sin_port=htons(port);
    nRecLen = sizeof(client_addr);
    INFO_LOG("success to create network send thread with destination %s:%d", ip.c_str(), port);

    context_ = context;
    dvpp_.InitResource(stream);

    thread sender_thread(TargetSendThreadFunction, (void*)this);
    sender_thread.detach();

    return SUCCESS;
}

void TargetSender::TargetSendThreadFunction(void* senderSelf)
{
    TargetSender* thisPtr = (TargetSender*)senderSelf;
    thisPtr->SetAclContext();
    SendData data;
    while(true)
    {
        data = thisPtr->SendDataOutQueue();
        // INFO_LOG("send queue size: %d",thisPtr->GetQueueSize());
        // struct timeval begin;
        // struct timeval end;
        // gettimeofday(&begin, NULL);
        thisPtr->Send(data);
        // gettimeofday(&end, NULL);
        // float time_cost = 1000 * (end.tv_sec - begin.tv_sec) + (end.tv_usec - begin.tv_usec) / 1000.000;
        // INFO_LOG("Send time: %fms", time_cost);
    }
}


void TargetSender::Send(SendData& data)
{
    
    vector<BBox> rlts = data.rlts;
    int image_id = data.image_id;
    cv::Mat bgr = data.bgr;

    if (bgr.empty()) {    // 如果没有传入图像就只发送结果，如果传入了就只发送图像
        SendResult(rlts, image_id);
    } else {
        SendJpeg(bgr, image_id);
    }
}

void TargetSender::SendJpeg(const cv::Mat& bgr, int image_id) {

    cv::Mat yuv;
    Utils::BGR2YUV_nv12(bgr, yuv);
    
    ImageData yuvImageData, jpegImageData;
    Utils::CopyImageDataToDvpp(yuvImageData, yuv);
    yuvImageData.format = PIXEL_FORMAT_YUV_SEMIPLANAR_420;
    yuvImageData.width = bgr.cols;
    yuvImageData.height = bgr.rows;
    Result ret = (Result)dvpp_.JpegE(jpegImageData, yuvImageData);
    if (ret == FAILED) {
        ERROR_LOG("jpg encode failed");
        return;
    }

    ImageFrameHead img_frm_head = {};
    // 生成图像帧头
    img_frm_head.img_id = image_id;
    img_frm_head.img_height = jpegImageData.height;
    img_frm_head.img_width = jpegImageData.width;

    time_t currentTime;
    time(&currentTime);
    struct tm *p;
    p = localtime(&currentTime);
    
    img_frm_head.hour = (uint8_t)p->tm_hour;
    img_frm_head.minute = (uint8_t)p->tm_min;
    img_frm_head.second = (uint8_t)p->tm_sec;
    
    // 计算CRC校验，除去帧头和CRC校验的4个字节
    unsigned short CRC = Utils::Crc16Fast((unsigned char*)&img_frm_head+2, sizeof(img_frm_head) - 4, 0);
    img_frm_head.crc=CRC;

    int img_frm_head_length = sizeof(img_frm_head);
    int img_frm_data_length = jpegImageData.size;
    int img_frm_length = img_frm_head_length + img_frm_data_length;  // 图像帧头和图像的总大小

    // 拷贝图像帧头和数据到缓冲区
    unsigned char img_frm[img_frm_length];
    memcpy(img_frm, &img_frm_head, img_frm_head_length);
    memcpy(img_frm+img_frm_head_length, jpegImageData.data.get(), img_frm_data_length);

    // 分包逐一构建UDP帧头发送
    unsigned short num_pack = (img_frm_length-1)/unit_length+1; //分包数

    
    UdpFrame frame = {0};
    frame.head = UDP_FRAME_HEAD;
    frame.id = UDP_FRAME_ID;
    frame.num = num_pack;
    for (unsigned short i=0; i<num_pack; i++) {

        unsigned char* img_pack = img_frm + i*unit_length;
        unsigned int img_pack_size;
        if(i<num_pack-1) {
            img_pack_size = unit_length;
        }
        else {
            img_pack_size = img_frm_length - i*unit_length;
        }
        frame.len = kBytesOfUdpFrameHead + img_pack_size;
        frame.seq = i;
        memcpy(frame.frm_body.uint8, img_pack, img_pack_size);
        unsigned char check=0;
        unsigned char* frame_ptr = (unsigned char*)&frame;
        for (unsigned int j=8; j < frame.len-4; j++) {
            check += frame_ptr[j];
        }
        // 这样操作的原因是struct的遵照最长的数据长度对齐，而且TAIL只有3个字节
        frame_ptr[frame.len-4] = (UDP_FRAME_TAIL)     & 0xFF;
        frame_ptr[frame.len-3] = (UDP_FRAME_TAIL>>8)  & 0xFF;
        frame_ptr[frame.len-2] = (UDP_FRAME_TAIL>>16) & 0xFF;
        frame_ptr[frame.len-1] = check;

        // 最多重试3次
        uint8_t retry = 0;
        while (SendPackage(frame_ptr, frame.len)==FAILED && retry < 3) {
            usleep(1);
            retry++;
        }
        if (retry == 3) {
            ERROR_LOG("image %d send timeout", image_id);
            break;
        }
        // usleep(10);   // 百兆网线下请设为250，千兆网下请设为25，typec连接下请注释该行
    }
}

void TargetSender::SendImage(cv::Mat& bgr, int image_id) {

    ImageFrameHead img_frm_head = {};
    // 生成图像帧头
    img_frm_head.img_id = image_id;
    img_frm_head.img_height = bgr.rows;
    img_frm_head.img_width = bgr.cols;

    time_t currentTime;
    time(&currentTime);
    struct tm *p;
    p = localtime(&currentTime);
    
    img_frm_head.hour = (uint8_t)p->tm_hour;
    img_frm_head.minute = (uint8_t)p->tm_min;
    img_frm_head.second = (uint8_t)p->tm_sec;
    
    // 计算CRC校验，除去帧头和CRC校验的4个字节
    unsigned short CRC = Utils::Crc16Fast((unsigned char*)&img_frm_head+2, sizeof(img_frm_head) - 4, 0);
    img_frm_head.crc=CRC;

    int img_frm_head_length = sizeof(img_frm_head);
    int img_frm_data_length = bgr.rows*bgr.cols*bgr.elemSize();
    int img_frm_length = img_frm_head_length + img_frm_data_length;  // 图像帧头和图像的总大小

    // 拷贝图像帧头和数据到缓冲区
    unsigned char img_frm[img_frm_length];
    memcpy(img_frm, &img_frm_head, img_frm_head_length);
    memcpy(img_frm+img_frm_head_length, bgr.data, img_frm_data_length);

    // 分包逐一构建UDP帧头发送
    unsigned short num_pack = (img_frm_length-1)/unit_length+1; //分包数

    
    UdpFrame frame = {0};
    frame.head = UDP_FRAME_HEAD;
    frame.id = UDP_FRAME_ID;
    frame.num = num_pack;
    for (unsigned short i=0; i<num_pack; i++) {

        unsigned char* img_pack = img_frm + i*unit_length;
        unsigned int img_pack_size;
        if(i<num_pack-1) {
            img_pack_size = unit_length;
        }
        else {
            img_pack_size = img_frm_length - i*unit_length;
        }
        frame.len = kBytesOfUdpFrameHead + img_pack_size;
        frame.seq = i;
        memcpy(frame.frm_body.uint8, img_pack, img_pack_size);
        unsigned char check=0;
        unsigned char* frame_ptr = (unsigned char*)&frame;
        for (unsigned int j=8; j < frame.len-4; j++) {
            check += frame_ptr[j];
        }
        // 这样操作的原因是struct的遵照最长的数据长度对齐，而且TAIL只有3个字节
        frame_ptr[frame.len-4] = (UDP_FRAME_TAIL)     & 0xFF;
        frame_ptr[frame.len-3] = (UDP_FRAME_TAIL>>8)  & 0xFF;
        frame_ptr[frame.len-2] = (UDP_FRAME_TAIL>>16) & 0xFF;
        frame_ptr[frame.len-1] = check;

        // 最多重试3次
        uint8_t retry = 0;
        while (SendPackage(frame_ptr, frame.len)==FAILED && retry < 3) {
            usleep(1);
            retry++;
        }
        if (retry == 3) {
            ERROR_LOG("image %d send timeout", image_id);
            break;
        }
        // usleep(10);   // 百兆网线下请设为250，千兆网下请设为25，typec连接下请注释该行
    }
}

void TargetSender::SendResult(vector<BBox>& rlts, int image_id) {
    UdpFrame frame = {0};
    int size = rlts.size();
    int lenOfOneResult = 20; // tlx(2), tly(2), brx(2), bry(2), trk_id(4), cls_id(4), score(4)
    int lenOfResultHead = 10;  // head(2), len(1), command(1), id(4), crc(2)
    
    frame.head = UDP_FRAME_HEAD;
    frame.len = kBytesOfUdpFrameHead + lenOfResultHead + size * lenOfOneResult;
    frame.id = UDP_FRAME_ID;
    frame.seq = 0;
    frame.num = 1;
    frame.tail = UDP_FRAME_TAIL;

    frame.frm_body.uint8[0] = 0x5A;   // 帧头
    frame.frm_body.uint8[1] = 0x4A;
    frame.frm_body.uint8[2] = size;
    frame.frm_body.uint8[3] = 0xEF;    // 结果帧
    frame.frm_body.uint32[1] = image_id;  // 图像id


    for(int i = 0; i < size; i++)
    {
        int offset = i * lenOfOneResult/2;
        frame.frm_body.uint16[4 + 0 + offset] = (uint16_t)(rlts[i].rect.tl().x);
        frame.frm_body.uint16[4 + 1 + offset] = (uint16_t)(rlts[i].rect.tl().y);
        frame.frm_body.uint16[4 + 2 + offset] = (uint16_t)(rlts[i].rect.br().x);
        frame.frm_body.uint16[4 + 3 + offset] = (uint16_t)(rlts[i].rect.br().y);
        frame.frm_body.uint32[4 + 0 + offset/2] = (uint32_t)(rlts[i].trk_id);
        frame.frm_body.uint32[4 + 1 + offset/2] = (uint32_t)(rlts[i].cls_id);
        frame.frm_body.uint32[4 + 2 + offset/2] = (uint32_t)(rlts[i].score * 100);
    }

    unsigned char buffer[frame.len];
    memcpy(buffer, &frame, frame.len);
    unsigned char check = 0;
    // 不连帧头和帧尾 -4
    unsigned short crc = Utils::Crc16Fast(frame.frm_body.uint8+2, lenOfResultHead-4 + size * lenOfOneResult, 0);

    buffer[24 + size * lenOfOneResult] = crc;
    buffer[25 + size * lenOfOneResult] = crc >> 8;

    for(int i = 8; i < 26 + size * lenOfOneResult; i++)
    {
        check +=  buffer[i];
    }
    buffer[29 + size * lenOfOneResult] = check;
    buffer[26 + size * lenOfOneResult] = frame.tail;
    buffer[27 + size * lenOfOneResult] = frame.tail >> 8;
    buffer[28 + size * lenOfOneResult] = frame.tail >> 16;
    
    Result ret = SendPackage(buffer, frame.len);
    if (ret != SUCCESS) {
        ERROR_LOG("Send Frame ERROR");
    }
}

Result TargetSender::SendPackage(unsigned char data[], int size) {
    size_t ret;
    ret = sendto(clifd, data, size, MSG_NOSIGNAL, (struct sockaddr*)&client_addr, nRecLen);
        // cout << ret << " " << size << endl;

    if((int)ret == -1)
    {
        ERROR_LOG("send error %s", strerror(errno));
        return FAILED;
    }
    return SUCCESS;
}

Result TargetSender::SendDataEnQueue(vector<BBox>& resultData, int image_id, cv::Mat& bgr) {
    vector<BBox> bbox(resultData);
    SendData data;
    data = {bbox, image_id-1, bgr};

    for (int count = 0; count < kFrameEnQueueRetryTimes; count++) {
        if (SendDataQueue_.Push(data)) 
            return SUCCESS;
        usleep(kReadQueueOpWait); 
    }
    ATLAS_LOG_ERROR("sender %d lost data for queue full", 
	                image_id);

    return FAILED;
}

Result TargetSender::SendDataEnQueue(vector<BBox>& resultData, int image_id) {
    cv::Mat emptyMat(0, 0, CV_8UC3, cv::Scalar(0, 0, 0));
    return SendDataEnQueue(resultData, image_id, emptyMat);
}

SendData TargetSender::SendDataOutQueue() {
    return SendDataQueue_.PopAndWait();
}


AtlasError TargetSender::SetAclContext() {
    if (context_ == nullptr) {
        ATLAS_LOG_ERROR("TargetSender context is null");
        return ATLAS_ERROR_SET_ACL_CONTEXT;
    }
    
    aclError ret = aclrtSetCurrentContext(context_);
    if (ret != ACL_ERROR_NONE) {
        ATLAS_LOG_ERROR("TargetSender set context failed, error: %d", ret);
        return ATLAS_ERROR_SET_ACL_CONTEXT;
    }
    
    return ATLAS_OK;   
}